#ifndef ARAL_INTERFACE_SCENE_HPP
#define ARAL_INTERFACE_SCENE_HPP

#include "aral/data_structure_definition.hpp"
#include "aral/robot_utility.hpp"
#include "aral/robot_model.hpp" 
#include "aral/robot_solver.hpp"
#include "aral/robot_state.hpp"
#include "aral/robot_controller.hpp"
#include "aral/robot_planner.hpp"
#include "aral/robot_task.hpp"

namespace ARAL::interface {

/**
 * @class Scene
 * @brief 机器人算法库的顶层接口, 代表一个独立的机器人"场景"(Scene)或"世界"(World)实例。
 *        前景是机器人，可以添加多个机器人，每个机器人可以有多个状态，每个状态可以有多个控制器，任务管理器可以管理多个规划器。
 *        背景是环境模型，环境中可以添加多个物体(Object). 
 * 
 * @details
 *  - **全局场景**: 每个 Scene 实例都拥有一个唯一的全局世界坐标系。所有机器人和环境物体都存在于此场景中。
 *  - **工厂模式**: Scene 作为所有机器人算法模块(模型、求解器、状态、控制器、规划器、任务等)的顶层工厂和管理器, 负责创建和管理这些模块的实例。
 *  - **Utility**: 负责管理算法库的特性，如日志等级、参数配置或者独立于机器人的通用算法和工具
 *  - **Model/Solver/State/Controller**: 构成单个机器人的基础能力层。
 *  - **Planner**: 定义具体运动目标的规划任务层, 负责协调机器人间的交互，如碰撞检测和同步运动, 负责路径生成。
 *  - **Task**: 管理和调度Planner的工作流执行层, 负责执行逻辑和状态管理。
 * 
 * @b 使用流程:
 * @code
 * // 1. 创建一个全局唯一的场景实例
 * auto scene = CreateARALScene();
 * 
 * // 2. 向场景中添加一个或多个机器人
 * auto model_handle_A = scene->rlCreateRobotModel("robot_A", "/path/A/");
 * auto model_handle_B = scene->rlCreateRobotModel("robot_B", "/path/B/");
 * 
 * // 3. 获取机器人模型指针并创建依赖模块
 * auto model_A = scene->rlGetRobotModel(model_handle_A);
 * auto solver_A = scene->rlCreateRobotSolver(model_A);
 * auto state_A = scene->rlCreateRobotState(solver_A);
 * 
 * auto model_B = scene->rlGetRobotModel(model_handle_B);
 * auto solver_B = scene->rlCreateRobotSolver(model_B);
 * auto state_B = scene->rlCreateRobotState(solver_B);
 * 
 * // 4. 根据需求创建规划器:
 * //    - 方案A: 异步/同步控制(走多个轨迹)
 * auto planner_A = scene->rlCreateRobotPlanner({state_A});
 * auto planner_B = scene->rlCreateRobotPlanner({state_B});
 * 
 * //    - 方案B: 协调控制（走一个轨迹）
 * auto sync_planner = scene->rlCreateRobotPlanner({state_A, state_B});
 * 
 * // 5. 使用任务模块编排复杂任务
 * auto task_manager = scene->rlCreateRobotTask({sync_planner});
 * @endcode
 */
class Scene 
{
public:
    /**
     * @brief 创建通用算法实例 (例如：坐标变换、姿态计算等)。
     * @return 返回通用算法模块的智能指针。
     */
    virtual UtilityPtr rlGetUtility() = 0;

    /**
     * @brief 在当前场景中创建一个新的机器人模型实例及其配套的求解器。
     * 
     * @note **设计决策: 返回句柄而非直接返回智能指针**
     * @param robot_name 机器人名称 (场景支持同名的机器人，但ModelHandle唯一)。
     * @param path 机器人模型文件(URDF/SRDF)所在的根目录路径。
     * @return 返回该机器人模型的句柄(Handle)。如果创建失败, 返回值 < 0。
     */
    virtual ModelHandle rlCreateRobotModel(const std::string& robot_name, const std::string& path) = 0;

    /**
     * @brief 从场景中销毁一个机器人模型及其所有依赖的子模块 (级联删除)。
     * 
     * @details
     *   这是一个级联销毁操作。销毁一个`Model`实例会导致所有依赖于它的下游模块被自动销-毁，以防止悬挂指针和资源泄漏。
     *   销毁链条如下：
     *   `Model` -> `Solver` -> `State` -> `Controller` & `Planner` -> `Task`
     * 
     *   例如，调用 `rlDestroyRobotModel(model_A_handle)` 将会：
     *   1. 找到所有使用 `model_A` 创建的 `Solver` 实例。
     *   2. 找到所有使用这些 `Solver` 创建的 `State` 实例。
     *   3. 找到所有使用这些 `State` 创建的 `Controller` 和 `Planner` 实例。
     *   4. 找到所有使用这些 `Planner` 创建的 `Task` 实例。
     *   5. 按照相反的顺序安全地销毁所有这些关联的实例，最后销毁 `model_A` 本身。
     * 
     * @warning
     *   调用此函数后，之前获取的所有与该模型相关的 `SolverPtr`, `StatePtr`, `PlannerPtr` 等智能指针将失效。
     *   继续使用这些指针将导致未定义行为。
     * @param handle 需要销毁的机器人模型的句柄。
     * @return 成功返回0, 失败返回错误码 (例如，句柄无效)。
     */
    virtual int rlDestroyRobotModel(const ModelHandle& handle) = 0;

    /**
     * @brief 获取当前场景中所有机器人模型的句柄列表。
     * @return 返回机器人模型句柄的向量。
     */
    virtual std::vector<ModelHandle> rlGetAllRobotModelHandles() = 0;

    /**
     * @brief 根据句柄获取机器人模型的智能指针。
     * @param handle 机器人模型的句柄。
     * @return 返回机器人模型的智能指针, 如果句柄无效则返回 nullptr。
     */
    virtual ModelPtr rlGetRobotModel(const ModelHandle& handle) = 0;

    /**
     * @brief [高级接口] 替换指定句柄的机器人模型。
     * @param handle 机器人模型的句柄。
     * @param model 新的机器人模型智能指针。
     * @return 返回被替换的机器人模型智能指针。
     */
    virtual int rlSetRobotModel(const ModelHandle& handle, const ModelPtr& model) = 0;

    /**
     * @brief 根据句柄获取机器人求解器的智能指针。
     * @param handle 目标机器人模型的句柄。
     * @return 返回求解器的智能指针。
     */
    virtual SolverPtr rlGetRobotSolver(const ModelHandle& handle) = 0;

    /**
     * @brief 为指定的机器人模型创建一个新的、独立的“状态”实例。
     * 
     * @details
     *   一个机器人模型(`Model`)可以拥有多个相互隔离的状态(`State`)实例。
     *   这对于管理和比较机器人的不同状态非常有用，例如：
     *   - **仿真与现实**: 一个`State`用于跟踪真实硬件的当前状态，另一个用于仿真预测。
     *   - **多重假设**: 同时模拟多种不同外部力或控制策略下的机器人行为。
     * 
     * @b 使用示例:
     * @code
     * // 为同一个机器人模型创建两个独立的状态
     * auto model_h = scene->rlCreateRobotModel("robot_A", "/path/A/");
     * 
     * // 状态1: 用于模拟
     * auto sim_state_h = scene->rlCreateRobotState(model_h, "simulation_state");
     * 
     * // 状态2: 用于连接真实硬件
     * auto real_state_h = scene->rlCreateRobotState(model_h, "real_hardware_state");
     * 
     * // 这两个状态实例可以被独立地更新和查询，互不影响
     * auto sim_state_ptr = scene->rlGetRobotState(sim_state_h);
     * auto real_state_ptr = scene->rlGetRobotState(real_state_h);
     * @endcode
     * 
     * @param handle 目标机器人模型的句柄。
     * @param state_name (可选) 为新创建的状态实例指定一个唯一的名称，便于管理。
     * @return 返回新创建的状态实例的句柄。
     */
    virtual StateHandle rlCreateRobotState(const ModelHandle& handle, const std::string& state_name) = 0;

    /**
     * @brief 根据句柄获取机器人状态的智能指针。
     * @param handle 机器人状态的句柄。
     * @return 返回机器人状态的智能指针, 如果句柄无效则返回 nullptr。
     */
    virtual StatePtr rlGetRobotState(const StateHandle& handle) = 0;

    /**
     * @brief 创建机器人控制器。
     * @param handle 机器人状态机的智能指针。
     * @return 返回机器人控制器智能指针。
     */
    virtual ControllerPtr rlCreateRobotController(const StateHandle& handle, const std::string& controller_name) = 0;

    /**
     * @brief 创建机器人轨迹规划器，并定义其中每个机器人的角色。
     *
     * @details
     *   一个`Planner`实例代表一个独立的“规划任务”。此接口允许精确控制
     *   规划器如何处理其管理的每个机器人。每个机器人的角色由 `PlannerRole` 枚举定义：
     *
     *   - `MASTER`: 规划器将为此机器人生成运动轨迹。在一个协同规划任务中，必须有且仅有一个`MASTER`。
     *   - `SLAVE`: 规划器将为此机器人生成与同一规划器中的`MASTER`相协调的运动轨迹。
     *   - `REFERENCE`: 此机器人的运动不由本规划器生成，而是作为外部参考，用于生成`SLAVE`的运动。
     *
     * @param robots 一个`RobotInPlanner`的向量，定义了所有参与此规划的机器人及其角色。
     * @param planner_name 为规划器指定的唯一名称。
     * @return 返回机器人规划器句柄。如果创建失败, 返回值 < 0。
     */
    virtual PlannerHandle rlCreateRobotPlanner(const std::vector<RobotRole>& robots, const std::string& planner_name) = 0;

    /**
     * @brief 根据句柄获取机器人规划器的智能指针。
     * @param handle 机器人规划器的句柄。
     * @return 返回机器人规划器的智能指针, 如果句柄无效则返回 nullptr。
     */
    virtual PlannerPtr rlGetRobotPlanner(const PlannerHandle& handle) = 0;

    /**
     * @brief 创建机器人任务管理器(Task)，如果有多个task，软件层负责管理。
     * 
     * @details
     *   一个`Task`实例是一个有状态的执行管理器，负责根据预设的依赖关系来启动、暂停、恢复和停止一个或多个`Planner`实例。
     *   如果一组规划任务之间存在执行依赖（例如，A任务完成后才能开始B任务），那么它们应该被同一个`Task`实例所管理。
     *   
     *   **核心职责**:
     *   - **状态管理**: 跟踪任务的整体状态（例如 `RUNNING`, `PAUSED`, `COMPLETED`）。
     *   - **工作流控制**: 根据用户定义的依赖关系，按顺序或并行地执行`Planner`。
     *   - **统一控制**: 提供`start()`, `pause()`, `resume()`, `stop()`等接口，以原子方式控制整个任务流。
     * 
     *   **场景1: 顺序依赖任务 (时间上依赖)**
     *   规划器A完成运动后，规划器B才能开始。
     *   @code
     *   // 为机器人A和B分别创建独立的规划器句柄
     *   auto planner_A_h = scene->rlCreateRobotPlanner({state_A});
     *   auto planner_B_h = scene->rlCreateRobotPlanner({state_B});
     * 
     *   // 获取规划器指针并添加运动
     *   auto planner_A = scene->rlGetRobotPlanner(planner_A_h);
     *   planner_A->addMoveLine(target_A);
     * 
     *   auto planner_B = scene->rlGetRobotPlanner(planner_B_h);
     *   planner_B->addMoveLine(target_B);
 * 
     *   // 创建一个Task来管理这两个有依赖关系的规划器
     *   auto sequential_task = scene->rlCreateRobotTask({planner_A_h, planner_B_h}, "sequential_task");
 * 
     *   // 在Task内部定义执行依赖：B依赖于A
     *   // (注意: 这需要Task类提供类似setDependency的接口)
     *   sequential_task->setDependency(planner_B_h, {planner_A_h}); 
 * 
     *   // 启动任务，Task会自动先执行A，再执行B
     *   sequential_task->start();
     *   @endcode
     * 
     *   **场景2: 并行同步任务 (空间上依赖)**
     *   规划器A和规划器B同时开始执行各自的运动，同时在路径上需要协同。
     *   @code
     *   // 为机器人A和B分别创建独立的规划器句柄
     *   auto planner_A_h = scene->rlCreateRobotPlanner({state_A});
     *   auto planner_B_h = scene->rlCreateRobotPlanner({state_B});
     * 
     *   // 获取规划器指针并添加运动
     *   auto planner_A = scene->rlGetRobotPlanner(planner_A_h);
     *   planner_A->addMoveLine(target_A);
 * 
     *   auto planner_B = scene->rlGetRobotPlanner(planner_B_h);
     *   planner_B->addMoveLine(target_B);
 * 
     *   // 创建一个Task来管理这两个规划器
     *   auto parallel_task = scene->rlCreateRobotTask({planner_A_h, planner_B_h}, "parallel_task");
 * 
     *   // 在Task内部定义为并行执行 (默认或通过API设置)
     *   // parallel_task->setExecutionMode(PARALLEL);
 * 
     *   // 启动任务，Task会同时启动A和B
     *   parallel_task->start();
     *   @endcode
     * 
     * **场景3: 并行独立任务 (时间上独立)**
     *   规划器A和规划器B同时开始执行各自的运动，但是不需要同步。
     *   @code
     *   // 为机器人A和B分别创建独立的规划器句柄
     *   auto planner_A_h = scene->rlCreateRobotPlanner({state_A});
     *   auto planner_B_h = scene->rlCreateRobotPlanner({state_B});
     *
     *   // 获取规划器指针并添加运动
     *   auto planner_A = scene->rlGetRobotPlanner(planner_A_h);
     *   planner_A->addMoveLine(target_A);
 * 
     *   auto planner_B = scene->rlGetRobotPlanner(planner_B_h);
     *   planner_B->addMoveLine(target_B);
 * 
     *   // 创建两个Task来管理这两个规划器
     *   auto independent_task_A = scene->rlCreateRobotTask({planner_A_h}, "task_A");
     *   auto independent_task_B = scene->rlCreateRobotTask({planner_B_h}, "task_B");
 * 
 * 
     *   // 启动任务，同时启动A和B
     *   independent_task_A->start();
     *   independent_task_B->start();
     *   @endcode
     * 
     * @param handles 一个或多个机器人规划器的句柄列表，它们共同组成一个可执行的任务。
     * @return 返回机器人任务管理器智能指针。
     */
    virtual TaskPtr rlCreateRobotTask(const std::vector<PlannerHandle>& handles, const std::string& task_name) = 0;

protected:
    /**
     * @brief 虚析构函数，确保多态删除时资源正确释放。
     */
    virtual ~Scene() = default;
};
typedef std::shared_ptr<Scene> ScenePtr;
}   //namespace ARAL::interface

extern "C" {
    /**
     * @brief 创建机器人算法库的场景实例 (工厂模式唯一入口)。
     * @param scene_name 为场景指定唯一名称。
     * @details
     *  - **独立场景**: 每个调用都会返回一个全新的、独立的算法场景(ARAL::interface::Scene)实例。
     *  - **并行处理**: 不同的场景实例之间完全隔离，互不影响，可以用于并行处理不同的机器人工作单元。
     *  - **生命周期**: 用户应在程序生命周期开始时创建所需场景，并通过智能指针自动管理其生命周期，无需手动释放。
     * @return 算法库场景实例的智能指针。创建成功可以强制转换成ARAL::interface::ScenePtr; 如果创建失败, 返回 nullptr。
     */
    void* CreateARALScene(const std::string& scene_name);
}

#endif // ARAL_INTERFACE_SCENE_HPP